#ifndef _COSMMAP_H
#define _COSMMAP_H

#include <iostream>
#include <math.h>
#include "COsmCommon.h"
#include "CRawImage.h"
#include "CHeap.h"

#define MAP_WIDTH 300
#define MAP_HEIGHT 200

typedef struct{
    double distance;
    double azimut;
} map_segment;

/**
 * COsmMap class.
 * This class is used for parsing and saving Open Street Map.
 */
class COsmMap
{
public:
    /**
     * Constructor.
     * Constructor of class COsmMap.
     */
    COsmMap();
    
    /**
     * Destructor.
     */
    ~COsmMap(){};
    
    /**
     * Get COsmNode pointer with given ID.
     * @param id Node ID.
     * @see COsmNode
     * @return COsmNode index
     */
    long int getOsmNode(long int id);
    
    /**
     * Get COsmWay pointer with given ID.
     * @param id Way ID.
     * @see COsmWay
     * @return COsmWay index
     */
    long int getOsmWay(long int id);

    /**
     * Get COsmRelation pointer with given ID.
     * @param id Relation id.
     * @see COsmRelation
     * @return COsmRelation index
     */
    long int getOsmRelation(long int id);
    
    /**
     * Print map.
     * Print map into terminal.
     */
    void printMap();

    /**
     * Draw map.
     * Draw map into given image. Given position is also displayed.
     * @param image Image pointer.
     * @param lat Current position latitude
     * @param lon Current position longitude
     */
    void drawMap(CRawImage *image, double *lat, double *lon);

    /**
     * Find path.
     * Find path between two nodes.
     * @param start Start node index in nodes/ID/pointer
     * @param goal Goal node index in nodes/ID/pointer
     * @return false if path not exist
     */
    bool findPath(long int start, long int goal, std::vector<COsmNode > &path);
    bool findPathID(long int start,long int goal,std::vector<COsmNode > &path)
    {
        int s,g;
        bool ret = true;
        s = getOsmNode(start);
        g = getOsmNode(goal);
        if(g < 0)
        {
            std::cerr << "Goal node not exist" << std::endl;
            ret = false;
        }
        if(s < 0)
        {
            std::cerr << "Start node not exist" << std::endl;
            ret = false;
        }
        if(!ret) return false;
        else return findPath(s,g,path);
    }

    bool findPathPtr(COsmNodePtr start, COsmNodePtr goal, std::vector<COsmNode > &path)
    {
        long int s,g;
        bool ret = true;
        s = start->id;
        g = goal->id;
        if(g < 0)
        {
            std::cerr << "Goal node not exist" << std::endl;
            ret = false;
        }
        if(s < 0)
        {
            std::cerr << "Start node not exist" << std::endl;
            ret = false;
        }
        if(!ret) return false;
        else return findPathID(s,g,path);
    }

    /**
     * Predraw OSM map.
     * Predraw map to buffer. This map can be drawn to image. 
     */
    void predrawMap();

    COsmBoundsPtr bounds;                      /**< JOSM map bounds */
    std::vector<COsmNode > nodes;               /**< Vector with nodes */
    std::vector<COsmWay > ways;                 /**< Vector with ways */
    std::vector<COsmRelation > relations;       /**< Vector with relations */

private:
    /**
     * Draw point
     * Set pixels in drawBuffer like a square 3x3
     * @param x X-coordinate of point
     * @param y Y-coordinate of point
     * @param r RGB: red color
     * @param g RGB: green color
     * @param b RGB: blue color
     */
    void drawPoint(int x, int y, int r, int g, int b);

    /**
     * Draw line
     * Set pixels in drawBuffer like a line form [x1,y1] to [x2,y2]
     * @param x1 X-coordinate of first point of line
     * @param y1 Y-coordinate of first point of line
     * @param x2 X-coordinate of last point of line
     * @param y2 Y-coordinate of last point of line
     * @param r RGB: red color
     * @param g RGB: green color
     * @param b RGB: blue color
     */
    void drawLine(int x1, int y1, int x2, int y2, int r, int g, int b);

    unsigned char drawBuffer[3*MAP_WIDTH*MAP_HEIGHT];   /**< Buffer for map image. */

};
#endif
